clc
clear all
close all
width=1024;
height=768;
[ F, K,ps , badpoints,corrs, FFormatted, corrsFormatted,EFormatted,spacePts,widthm,heightm,FFormattedCLEAN,FCLEAN  ] = generateF( 0,0,1.5,40,0,5,0,0 ,width,height,0);
%[ F,corrs,K,height,width,siftoutdirname ] = generateFImages( 'wadham',0, 0 );
K{1,1}
w=width;
h=height;


s=size(F,2);
allvals=[];
K=[width 0 width/2; 0 width height/2; 0 0 1];

numsl=150;
% find foc1
solzarray=zeros(numsl,s);
xarray=zeros(numsl,1);
nuarray=zeros(s,1);

focalmin=10;
focalmax=width*3;
ycmin=0;
ycmax=height;

for k=1:10
    close all
    for i=1:s
        [temparay,xarr]= provideFamilySolution_matrixnorm(F{1,i},w,h,2,numsl,K,'foc1');
        solzarray(:,i)=temparay;
    end
    xarray=xarr;
    plot(xarr,solzarray);
    xlabel('foc2');ylabel('foc1');title('results for foc1');
    figure
    [f,xi] = ksdensity(reshape(solzarray,[],1));
    plot(xi,f);
    [~,I] = max(f);
    K(1,1)=xi(I);
    display(['focal length was ' num2str(K(1,1))]);
    
    nuarray=[];
    for i=1:s
        [tempvfoc,~]= provideFamilySolution_matrixnorm(F{1,i},w,h,2,1,K,'foc2');
        if(tempvfoc>focalmin && tempvfoc<focalmax)
            nuarray=[nuarray ; tempvfoc];
        end
        
    end
    
    K(2,2)=median(nuarray(nuarray>2));
    display(['focal length was ' num2str(K(2,2))]);
    
    %%%%%%%%%%% now optical center %%%%%%%%%%%%%%%%%%%%%%%%
    figure
    for i=1:s
        [temparay,xarr]= provideFamilySolution_matrixnorm(F{1,i},w,h,2,numsl,K,'xc');
        solzarray(:,i)=temparay;
    end
    xarray=xarr;
    plot(xarr,solzarray);
    xlabel('yc');ylabel('xc');title('results for xc');
    figure
    [f,xi] = ksdensity(reshape(solzarray,[],1));
    plot(xi,f);
    [~,I] = max(f);
    K(1,3)=xi(I);
    display(['xc was ' num2str(K(1,3))]);
    
    nuarray=[];
    for i=1:s
        [tempvfoc,~]= provideFamilySolution_matrixnorm(F{1,i},w,h,2,1,K,'yc');
        if(tempvfoc>ycmin && tempvfoc<ycmax)
            nuarray=[nuarray ; tempvfoc];
        end
    end
    
    K(2,3)=median(nuarray(nuarray>2));
    display(['yc ' num2str(K(2,3))]);
    
    K
end